﻿using System.Collections;
using System.Collections.Generic;

/// <summary>
/// unity 坐标系 
/// </summary>
public class TransformUtility  {

    public static FixedVec3 RotatePoint(FixedVec3 worldPoint, FixedVec3 origin, FixedVec3 axis,FixedFloat radian)
    {
        FixedVec3 relativePos = worldPoint - origin;

        ZMatrix4X4 r = GetRotateMatrix(axis * radian);
        FixedVec4 p = new FixedVec4(relativePos.x, relativePos.y, relativePos.z, 1);
        FixedVec4 temp = r * p;

        FixedVec3 res = origin + new FixedVec3(temp.x, temp.y, temp.z);
        //GameDebug.Log($"旋转矩阵 \n{r} \n乘以 向量{p} 得到：{temp}  返回结果:{res}");
        return res;
    }

   

    public static FixedVec3 GetRotation2(ZMatrix4X4 R)
    {
        FixedFloat rx = 0;
        FixedFloat ry = 0;
        FixedFloat rz = 0;
        if (FixedFloatMath.Cos(rx) == 0)// R[2, 3] != 1 && R[2, 3] != -1)
        {
            rx = -FixedFloatMath.Asin(R[2, 3]);

            FixedFloat a = R[2, 1] / FixedFloatMath.Cos(rx);
            FixedFloat b = R[2, 2] / FixedFloatMath.Cos(rx);
            rz = FixedFloatMath.Atan2(a, b);


            FixedFloat c = R[1, 3] / FixedFloatMath.Cos(ry);
            FixedFloat d = R[3, 3] / FixedFloatMath.Cos(ry);
            ry = FixedFloatMath.Atan2(c, d);

            GameDebug.Log($"R31:{R[2, 3]}   ry:{ry}  rx:{rx}  rz:{rz}");
        }
        else if (R[2, 3] < 0) //== -1)
        {
            ry = 0;
            rx = FixedFloatMath.PI_Half;
            rz = ry + FixedFloatMath.Atan2(R[1, 2] , R[1, 1]);
        }
        else // (R[3, 1] == 1)
        {
            ry = 0;
            rx =  -FixedFloatMath.PI_Half;
            rz = ry + FixedFloatMath.Atan2(-R[1, 2], -R[1, 1]);
        }
        return new FixedVec3(ClampRad(rx), ClampRad(ry), ClampRad(rz));
    }

    public static FixedVec3 GetRotation(ZMatrix4X4 R)
    {
        FixedFloat rx = 0;
        FixedFloat ry = 0;
        FixedFloat rz = 0;
        
        FixedFloat a = R[2, 1];
        FixedFloat b = R[2, 2];
        rz = FixedFloatMath.Atan2(a, b);

        rx = FixedFloatMath.Atan2(-R[2, 3], FixedFloatMath.Sqrt(a*a+b*b));
        
        FixedFloat c = R[1, 3] ;
        FixedFloat d = R[3, 3] ;
        ry = FixedFloatMath.Atan2(c, d);
        
        return new FixedVec3(ClampRad(rx), ClampRad(ry), ClampRad(rz));
    }

    /// <summary>
    /// Unity的矩阵相乘顺序是y ,x ,z
    /// </summary>
    /// <param name="rotate"></param>
    /// <returns></returns>
    public static ZMatrix4X4 GetRotateMatrix(FixedVec3 rotate)
    {
        ZMatrix4X4 zM = GetRotateZMatrix(rotate.z);
        ZMatrix4X4 yM = GetRotateYMatrix(rotate.y);
        ZMatrix4X4 xM = GetRotateXMatrix(rotate.x);
        
        ZMatrix4X4 r = yM * xM * zM;
        // GameDebug.Log($"矩阵 \n{r} \n乘以 矩阵{yM}\n乘以 矩阵\n{yM} 得到：\n{r} ");
        return r;
    }
    
    public static ZMatrix4X4 GetRotateXMatrix(float alpha)
    {
        while (alpha < 0)
            alpha += FixedFloatMath.PI_Two;

        alpha %= FixedFloatMath.PI_Two; 

        ZMatrix4X4 RotateXMatrix = new ZMatrix4X4();
        RotateXMatrix[1, 1] = 1;
        RotateXMatrix[2, 2] = FixedFloatMath.Cos(alpha);
        RotateXMatrix[2, 3] = -FixedFloatMath.Sin(alpha);
        RotateXMatrix[3, 2] = FixedFloatMath.Sin(alpha);
        RotateXMatrix[3, 3] = FixedFloatMath.Cos(alpha);
        RotateXMatrix[4, 4] = 1;
        return RotateXMatrix;
    }

    public static ZMatrix4X4 GetRotateYMatrix(float alpha)
    {
        while (alpha < 0)
            alpha += FixedFloatMath.PI_Two;

        ZMatrix4X4 RotateYMatrix = new ZMatrix4X4();
        RotateYMatrix[1, 1] = FixedFloatMath.Cos(alpha);
        RotateYMatrix[1, 3] = FixedFloatMath.Sin(alpha);
        RotateYMatrix[2, 2] = 1;
        RotateYMatrix[3, 1] = -FixedFloatMath.Sin(alpha);
        RotateYMatrix[3, 3] = FixedFloatMath.Cos(alpha);
        RotateYMatrix[4, 4] = 1;
        return RotateYMatrix;
    }

    public static ZMatrix4X4 GetRotateZMatrix(float alpha)
    {
        while (alpha < 0)
            alpha += FixedFloatMath.PI_Two;

        ZMatrix4X4 RotateYMatrix = new ZMatrix4X4();
        RotateYMatrix[1, 1] = FixedFloatMath.Cos(alpha);
        RotateYMatrix[1, 2] = -FixedFloatMath.Sin(alpha);
        RotateYMatrix[2, 1] = FixedFloatMath.Sin(alpha);
        RotateYMatrix[2, 2] = FixedFloatMath.Cos(alpha);
        RotateYMatrix[3, 3] = 1;
        RotateYMatrix[4, 4] = 1;
        return RotateYMatrix;
    }

    
    public static FixedVec3 Deg2Rad(FixedVec3 deg)
    {
        return new FixedVec3(Deg2Rad(deg.x), Deg2Rad(deg.y), Deg2Rad(deg.z));
    }


    public static FixedFloat Deg2Rad(FixedFloat deg)
    {
        FixedFloat rad = deg * FixedFloatMath.Deg2Rad;
        rad = ClampRad(rad);
        return rad;
    }

    public static FixedFloat ClampRad(FixedFloat rad)
    {
        while (rad < 0)
            rad += FixedFloatMath.PI_Two;

        rad %= FixedFloatMath.PI_Two;
        return rad;
    }
}


///// <summary>
///// 右手坐标系
///// </summary>
//public class RightHandTransformUtility
//{
    
//    public static FixedVec3 GetRotation(ZMatrix4X4 R)
//    {
//        FixedFloat rx = 0;
//        FixedFloat ry = 0;
//        FixedFloat rz = 0;

//        FixedFloat a = R[2, 1];
//        FixedFloat b = R[2, 2];
//        rz = FixedMath.Atan2(a, b);

//        rx = FixedMath.Atan2(-R[2, 3], FixedMath.Sqrt(a * a + b * b));

//        FixedFloat c = R[1, 3];
//        FixedFloat d = R[3, 3];
//        ry = FixedMath.Atan2(c, d);

//        return new FixedVec3(ClampRad(rx), ClampRad(ry), ClampRad(rz));
//    }
    
//    public static ZMatrix4X4 GetRotateMatrix(FixedVec3 rotate)
//    {
//        ZMatrix4X4 zM = GetRotateZMatrix(rotate.z);
//        ZMatrix4X4 yM = GetRotateYMatrix(rotate.y);
//        ZMatrix4X4 xM = GetRotateXMatrix(rotate.x);

//        ZMatrix4X4 r = zM * yM * xM;// xM * yM * zM; //
//        // GameDebug.Log($"矩阵 \n{r} \n乘以 矩阵{yM}\n乘以 矩阵\n{yM} 得到：\n{r} ");
//        return r;
//    }

//    public static ZMatrix4X4 GetRotateXMatrix(float alpha)
//    {
//        while (alpha < 0)
//            alpha += FixedMath.PI_Two;

//        alpha %= FixedMath.PI_Two;

//        ZMatrix4X4 RotateXMatrix = new ZMatrix4X4();
//        RotateXMatrix[1, 1] = 1;
//        RotateXMatrix[2, 2] = FixedMath.Cos(alpha);
//        RotateXMatrix[2, 3] = FixedMath.Sin(alpha);
//        RotateXMatrix[3, 2] = -FixedMath.Sin(alpha);
//        RotateXMatrix[3, 3] = FixedMath.Cos(alpha);
//        RotateXMatrix[4, 4] = 1;
//        return RotateXMatrix;
//    }

//    public static ZMatrix4X4 GetRotateYMatrix(float alpha)
//    {
//        while (alpha < 0)
//            alpha += FixedMath.PI_Two;

//        ZMatrix4X4 RotateYMatrix = new ZMatrix4X4();
//        RotateYMatrix[1, 1] = FixedMath.Cos(alpha);
//        RotateYMatrix[1, 3] = -FixedMath.Sin(alpha);
//        RotateYMatrix[2, 2] = 1;
//        RotateYMatrix[3, 1] = FixedMath.Sin(alpha);
//        RotateYMatrix[3, 3] = FixedMath.Cos(alpha);
//        RotateYMatrix[4, 4] = 1;
//        return RotateYMatrix;
//    }

//    public static ZMatrix4X4 GetRotateZMatrix(float alpha)
//    {
//        while (alpha < 0)
//            alpha += FixedMath.PI_Two;

//        ZMatrix4X4 RotateYMatrix = new ZMatrix4X4();
//        RotateYMatrix[1, 1] = FixedMath.Cos(alpha);
//        RotateYMatrix[1, 2] = FixedMath.Sin(alpha);
//        RotateYMatrix[2, 1] = -FixedMath.Sin(alpha);
//        RotateYMatrix[2, 2] = FixedMath.Cos(alpha);
//        RotateYMatrix[3, 3] = 1;
//        RotateYMatrix[4, 4] = 1;
//        return RotateYMatrix;
//    }


//    public static FixedVec3 Deg2Rad(FixedVec3 deg)
//    {
//        return new FixedVec3(Deg2Rad(deg.x), Deg2Rad(deg.y), Deg2Rad(deg.z));
//    }


//    public static FixedFloat Deg2Rad(FixedFloat deg)
//    {
//        FixedFloat rad = deg * FixedMath.Deg2Rad;
//        rad = ClampRad(rad);
//        return rad;
//    }

//    public static FixedFloat ClampRad(FixedFloat rad)
//    {
//        while (rad < 0)
//            rad += FixedMath.PI_Two;

//        rad %= FixedMath.PI_Two;
//        return rad;
//    }
//}
